import os
import sys
import yaml
#get_package_share_directory: 获取 ROS 2 包的共享目录路径，用于加载配置文件等资源。
from ament_index_python.packages import get_package_share_directory
#GroupAction: 用于将多个启动操作组合在一起执行。
from launch.actions import GroupAction
#Node: 用于启动 ROS 节点
from launch_ros.actions import Node
#LaunchConfiguration: 用于获取启动时传入的参数值。
#Command: 用于执行命令行命令
from launch.substitutions import LaunchConfiguration, Command
#将 rm_vision_bringup 包中的 launch 目录添加到 sys.path，这样可以方便地在脚本中引用该目录下的文件。
sys.path.append(os.path.join(get_package_share_directory('rm_vision_bringup'), 'launch'))

def generate_launch_description():

    from launch_ros.descriptions import ComposableNode
    from launch_ros.actions import ComposableNodeContainer
    from launch.actions import TimerAction, Shutdown
    from launch import LaunchDescription

    launch_params = yaml.safe_load(open(os.path.join(
    get_package_share_directory('rm_vision_bringup'), 'config', 'launch_params.yaml')))

    node_params = os.path.join(
    get_package_share_directory('rm_vision_bringup'), 'config', 'node_params.yaml')

    vision_attack_node = Node(
        package='vision_attacker',
        executable='vision_attacker_node',
        namespace='red_standard_robot1',
        parameters=[node_params, {"use_sim_time": True}],
        remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
    )

    bringup_cmd_group = GroupAction([
        #robot_state_publisher,
        ComposableNodeContainer(
            name='camera_detector_tracker_ballistics_container',
            namespace='red_standard_robot1',
            package='rclcpp_components',
            executable='component_container',
            remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
            composable_node_descriptions=[
                # ComposableNode(
                #     package='hik_camera',
                #     plugin='hik_camera::HikCameraNode',
                #     name='hik_camera_node',
                #     parameters=[node_params],
                #     extra_arguments=[{'use_intra_process_comms': True}]
                # ),
                ComposableNode(
                    package='armor_detector',
                    plugin='rm_auto_aim::ArmorDetectorNode',
                    name='armor_detector',
                    namespace='red_standard_robot1',
                    parameters=[node_params, {"use_sim_time": True}],
                    extra_arguments=[{'use_intra_process_comms': True}],
                    remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static'), ('image_raw', 'front_camera/image'), ('camera_info', 'front_camera/camera_info')],

                ),
                ComposableNode(
                    package='armor_tracker',
                    plugin='rm_auto_aim::ArmorTrackerNode',
                    namespace='red_standard_robot1',
                    name='armor_tracker',
                    parameters=[node_params, {"use_sim_time": True}],
                    extra_arguments=[{'use_intra_process_comms': True}],
                    remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
                )
            ],
            output='both',
            emulate_tty=True,
            ros_arguments=['--ros-args', '--log-level',
                           'armor_detector:=' + launch_params['detector_log_level']],
            on_exit=Shutdown(),
        ),
    ])

    return LaunchDescription([
        bringup_cmd_group,
        vision_attack_node
    ])


